#! /usr/bin/env python
'''
    这是硬件接口线程，该线程用于直接和下位机进行通信。
    该线程应该有一个数组用于表示所有向单片机发送的信号的内容。
    同时也应该在此定义和单片机通信时候每一位代表的含义
    为了实现解耦，应该提供一个字典实现数据到位置之间的转换
'''
from threading import Thread
from time import sleep
import rospy

#自定义类
class HWThread(Thread):


 '''
 和单片机的通信协议声明：
    on/off case:用于锁定/解锁单片机，需要有一个标志位来说明是否要解锁 (锁定：1)
    man/fcs case:用于手动控制单片机 需要四个位表示电机转速，四个位表示舵机角度，一个位表示是否发射（电机：2-5 舵机：6-9 射击：10）
    为了其他情况下的扩展（比如之后加入的距离传感器和激光，再预留4个标志）（备用：11-14）

    具体表达声明：
    1： 0代表上锁，1代表解锁
    2-5：+ 顺时针  - 逆时针 单位：转速
    6-9：角度（角度制） + 顺时针 - 逆时针
    10： 1：射击指令下达  0：射击指令未下达
    11-14：默认为0
 '''

 def __init__(self,queue,serial):
   Thread.__init__(self)
   #传入消息队列
   self.queue = queue
   self.serial = serial
   self.messageInit()



 def messageInit(self):
    #定义自身消息接口，这就是向下位机要发送的16进制数字信息
    #定义消息队列，其中均采用文件中配置的初始默认值。
    #在单片机初始启动的时候，处于锁定状态，并不会给电机和舵机发送任何指令，在on命令下达之后，单片机解除锁定状态，并按照这里的要求进行更新
    #因此要将这个发送数组的值都定义为设定好的初始值，这样才不会出错
     self.txMessageList = [
        0, #1:表示锁定
        0, #2/3/4/5:初始电机转速为0
        0,
        0,
        0,
        #servoADefault: 0.0
        #servoBDefault: 60.0
        #servoCDefault: 45.0
        #servoDDefault: -10.0
        float(rospy.get_param("servoADefault",0.0)),
        float(rospy.get_param("servoBDefault",60.0)),
        float(rospy.get_param("servoCDefault",45.0)),
        float(rospy.get_param("servoDDefault",-10.0)),
        0, #10:射击位
        0, #11-14:预留的位置
        0,
        0,
        0
     ]
     #这里用来定义一个字典表示信息到数组位置之间的转换   
     self.helper = {
        "lock":0,
        #轮子定义是按照逆时针方向，前进方向左手边第一个轮子定义为A，之后依次是B/C/D
        "wheelA":1,
        "wheelB":2,
        "wheelC":3,
        "wheelD":4,
        #舵机是自底向上定义，最下面为A，之后向上为B/C/D
        "servoA":5,
        "servoB":6,
        "servoC":7,
        "servoD":8,
        "shoot":9
     }
     #这是只翻译成一位十六进制的表，对应要翻译的索引
     self.translate2SingleHexTable = [self.helper["lock"],self.helper["shoot"]]

 def execute(self):
      print("Hardware Thread start!")
      while True:
         #时刻检查队列的更新情况
         self.update()
         #同时也要时刻检测串口传回的数据，这里暂时先留下
         self.examineSerial()
    #   self.epilogue()

 def run(self):
    self.execute()



    

 def update(self):
     #这里随时检测hwThreadManager向队列中发送的信息，如果有信息，那么就识别并且更新自身的消息队列，随后发送
     while(not self.queue.empty()):
         self.cmd = self.queue.get()
         #将进程的信息打包
         self.package(self.cmd)
         #将打包之后的信息从串口发送
         self.serialSend()
         #这个是每次发送之后的一些处理函数，比如停止射击等
         self.epilogue()

 def package(self,cmd):
     #这个函数用来判定上面调用信息的内容，并且更新要发送的数组
     '''
     接口：arm:机械臂  car:小车底盘  shoot:射击

        arm:
     float64 baseYawAngle
     float64 basePitchAngle
     float64 middlePitchAngle
     float64 upperPitchAngle


        car:
     float64 wheelA
     float64 wheelB
     float64 wheelC
     float64 wheelD
     '''
     if(cmd["name"] == "arm"):
         self.txMessageList[self.helper["servoA"]] = cmd["data"].baseYawAngle
         self.txMessageList[self.helper["servoB"]] = cmd["data"].basePitchAngle
         self.txMessageList[self.helper["servoC"]] = cmd["data"].middlePitchAngle
         self.txMessageList[self.helper["servoD"]] = cmd["data"].upperPitchAngle
     elif(cmd["name"] == "car"):
         self.txMessageList[self.helper["wheelA"]] = cmd["data"].wheelA
         self.txMessageList[self.helper["wheelB"]] = cmd["data"].wheelB
         self.txMessageList[self.helper["wheelC"]] = cmd["data"].wheelC
         self.txMessageList[self.helper["wheelD"]] = cmd["data"].wheelD
     elif(cmd["name"] == "shoot"):
         self.txMessageList[self.helper["shoot"]] = 1
     elif(cmd["name"] == "lock"):
         '''
         LockCmd: off
         UnlockCmd: on
         '''
         if(cmd["data"].data == "off"):
             #如果是上锁命令
             self.txMessageList[self.helper["lock"]] = 0
         elif(cmd["data"].data == "on"):
             #如果是解锁命令，那就解除锁定
             self.txMessageList[self.helper["lock"]] = 1

 def serialSend(self):
     #由于在列表中的信息都是用int或float表示的，而串口只能够发送16进制信息，因此需要对信息进行处理
     #和校验位
     self.sum = 0
     HexList = []
     for i in range(0,self.helper["shoot"]+1):
         HexList.extend(self.data2Hex(self.txMessageList[i],i))
     for _ in range(0,4):
         #都是0x00,这是预留的信息位
         HexList.extend(self.data2Hex(0,-1))
     HexList.extend(self.addExamine())

     #两个标志位，说明有数据到来
     #波特率不能太低，太低会造成延迟严重
     self.serial.write(0x7A)
     self.serial.write(0x5b)
     for i in HexList:
         self.serial.write(i)

     rospy.loginfo("原始信息：")
     print(self.txMessageList)
     rospy.loginfo("已发送信息：")
     print(HexList)

     

 def epilogue(self):
     self.txMessageList[self.helper["shoot"]] = 0

 def examineSerial(self):
     pass

 def addExamine(self):
     #用2组16进制数字表示求和
     numa = self.sum // 255
     numb = self.sum % 255
     print("和校验的数据为：")
     print(self.sum)
     return [self.int2Bytes(numa),self.int2Bytes(numb)]


 def data2Hex(self,data,index):
     #这是一个将数据转换成十六进制代码的函数
     #接收到数据后，根据翻译表，判断要翻译的位数
     if(index == -1):
         return [self.int2Bytes(data)]
     if(index in self.translate2SingleHexTable):
         return [self.int2Bytes(data)]
     else:
        symbol = -1
        #整数部分 0-255
        #小数部分 0-100 （即0.01-0.99，使用的时候将其除以100）
        #如果不在那个表中，证明要将数字拆分成三位，首先判断符号
        # print("此次转换的数字为：")
        # print(data)
        if(data > 0 or data == 0):
            #如果是正数,那么数值就是0
            symbol = 0
        else:
            symbol = 1
        #之后看这个数据的整数部分
        upperTemp = int(abs(data)) #将数据转换成整数，即丢掉小数部分，同时取绝对值
        lowerTemp = int(round(abs(data)-upperTemp,2)*100) #取小数部分，并且保留两位。乘以100变成整数
        #最后将这几个数字封装
        res = [self.int2Bytes(symbol),self.int2Bytes(upperTemp),self.int2Bytes(lowerTemp)]
        # print(res)
        return res

 def int2Bytes(self,num):
         if(num > 255):
             num = 255
             rospy.loginfo("[error]:数字越界")
         #由于每一个数字都是通过这个函数转换成那个要发送的数组，因此要进行和校验，需要在这里不断加上该位数
         self.sum += num
         temp = str(hex(num)[2:]) #这里得到一个十六进制的数字
         if(num < 16):
             temp = "0"+temp
        #  print("转换后的十六进制数字：")
        #  print(temp)
         return bytes.fromhex(temp)


